/**
  ******************************************************************************
  * @file    modbus.c
  * @brief   This file provides code for the configuration
  *          of all used GPIO pins.
  ******************************************************************************
  * @attention
  *
  ******************************************************************************
  */

/* Includes ------------------------------------------------------------------*/
#include "bsp.h"
#include "modbus.h"
#include "gv.h"
#include "tskcfg.h"
#include <string.h>

const uint8_t mb_local_staddr = MODBUS_LOCAL_STATION_ADDRESS;  /* 本机站号 */

Modbus_MemMapStru mbmmap;

const Modbus_MemoryMapStru mb_map = 
{
  .cosize   = sizeof(mbmmap.co),
  .disize   = sizeof(mbmmap.di),
  .irsize   = sizeof(mbmmap.ir)/sizeof(uint16_t),
  .hrsize   = sizeof(mbmmap.hr)/sizeof(uint16_t),
  .coils    = (uint8_t  *)&mbmmap.co,
  .disinput = (uint8_t  *)&mbmmap.di,
  .inputreg = (uint16_t *)&mbmmap.ir,
  .holdreg  = (uint16_t *)&mbmmap.hr,
};

static uint8_t modbus_01rd_coils(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *wr)
{
  uint8_t  i;
  uint16_t j;
  uint8_t  intbyte;
  uint8_t  remainbit;
  uint16_t pos;
  uint16_t startaddr;
  uint16_t quantity;

  if(inlen < (MODBUS_FRAME_MINLEN + 4))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);
  quantity  = BIGENDIAN_TO_U16(in[10], in[11]);

  if(((startaddr + quantity) > wr->cosize) || \
     (quantity > MODBUS_01RD_COILS_QUAMAX) || \
     (quantity < 1)) 
  {
    return 1;
  }

  intbyte   = quantity/8;
  remainbit = quantity%8;
  pos = startaddr;

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_01RD_COILS;
  if(remainbit != 0)
  {
    out[8] = intbyte+1;
  }
  else
  {
    out[8] = intbyte;
  }

  for(j=9, intbyte+=9; j<intbyte; j++)
  {
    out[j] = 0;
    
    for(i=0; i<8; i++)
    {
      if(wr->coils[pos++] != 0)
      {
        out[j] |= (0x01<<i);
      }
    }
  }

  out[j] = 0;

  for(i=0; i<remainbit; i++)
  {
    if(wr->coils[pos++] != 0)
    {
      out[j] |= (0x01<<i);
    }
  }

  if(remainbit != 0)
  {
    j++;
  }

  *outlen = j;

  j -= 6;

  /* modbus tcp 后面报文长度 */
  out[4] = (uint8_t)j;
  out[5] = (uint8_t)(j >> 8);

  return 0;
}

static uint8_t modbus_02rd_disinputs(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *ro)
{
  uint8_t  i;
  uint16_t j;
  uint8_t  intbyte;
  uint8_t  remainbit;
  uint16_t pos;
  uint16_t startaddr;
  uint16_t quantity;

  if(inlen < (MODBUS_FRAME_MINLEN + 4))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);
  quantity  = BIGENDIAN_TO_U16(in[10], in[11]);

  if(((startaddr + quantity) > ro->disize) || \
     (quantity > MODBUS_02RD_DISINPUTS_QUAMAX) || \
     (quantity < 1)) 
  {
    return 1;
  }

  intbyte   = quantity/8;
  remainbit = quantity%8;
  pos = startaddr;

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_02RD_DISINPUTS;
  if(remainbit != 0)
  {
    out[8] = intbyte+1;
  }
  else
  {
    out[8] = intbyte;
  }

  for(j=9, intbyte+=9; j<intbyte; j++)
  {
    out[j] = 0;

    for(i=0; i<8; i++)
    {
      if(ro->disinput[pos++] != 0)
      {
        out[j] |= (0x01<<i);
      }
    }
  }

  out[j] = 0;

  for(i=0; i<remainbit; i++)
  {
    if(ro->disinput[pos++] != 0)
    {
      out[j] |= (0x01<<i);
    }
  }

  if(remainbit != 0)
  {
    j++;
  }

  *outlen = j;

  j -= 6;

  out[4] = (uint8_t)j;
  out[5] = (uint8_t)(j >> 8);

  return 0;
}

static uint8_t modbus_03rd_holdreg(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *ro)
{
  uint16_t i;
  uint16_t pos;
  uint16_t startaddr;
  uint16_t quantity;

  if(inlen < (MODBUS_FRAME_MINLEN + 4))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);
  quantity  = BIGENDIAN_TO_U16(in[10], in[11]);

  if(((startaddr + quantity) > ro->hrsize) || \
     (quantity > MODBUS_03RD_HOLDREG_QUAMAX) || \
     (quantity < 1)) 
  {
    return 1;
  }

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_03RD_HOLDREG;
  out[8] = quantity<<1;

  pos = startaddr;

  for(i=0; i<quantity; i++)
  {
    out[(2*i)+9] = (uint8_t)(ro->holdreg[pos]>>8);
    out[(2*i)+10] = (uint8_t)ro->holdreg[pos];
    pos++;
  }

  *outlen = (2*i)+9;  /* 总长 */

  i = *outlen - 6;

  out[4] = (uint8_t)i;
  out[5] = (uint8_t)(i >> 8);

  return 0;
}

static uint8_t modbus_04rd_inputreg(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *ro)
{
  uint16_t i;
  uint16_t pos;
  uint16_t startaddr;
  uint16_t quantity;

  if(inlen < (MODBUS_FRAME_MINLEN + 4))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);
  quantity  = BIGENDIAN_TO_U16(in[10], in[11]);

  if(((startaddr + quantity) > ro->irsize) || \
     (quantity > MODBUS_04RD_INPUTREG_QUAMAX) || \
     (quantity < 1)) 
  {
    return 1;
  }

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_04RD_INPUTREG;
  out[8] = quantity<<1;

  pos = startaddr;

  for(i=0; i<quantity; i++)
  {
    out[(2*i)+9] = (uint8_t)(ro->inputreg[pos]>>8);
    out[(2*i)+10] = (uint8_t)ro->inputreg[pos];
    pos++;
  }

  *outlen = (2*i)+9;

  i = *outlen - 6;

  out[4] = (uint8_t)i;
  out[5] = (uint8_t)(i >> 8);

  return 0;
}

static uint8_t modbus_05wt_singlcoil(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *wro)
{
  uint16_t startaddr;

  if(inlen < (MODBUS_FRAME_MINLEN + 4))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);

  if(startaddr > wro->cosize)
  {
    return 1;
  }

  if((in[10] == 0xFFu) && (in[11] == 0x00u))
  {
    wro->coils[startaddr] = 1;
  }
  else if((in[10] == 0x00) && (in[11] == 0x00))
  {
    wro->coils[startaddr] = 0;
  }
  else
  {
    return 2;
  }

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[4] = 0x00;
  out[5] = 0x06;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_05WT_SINGLCOIL;
  out[8] = (uint8_t)(startaddr >> 8);
  out[9] = (uint8_t)startaddr;

  if(wro->coils[startaddr] != 0)
  {
    out[10] = 0xFFu;
  }
  else
  {
    out[10] = 0x00u;
  }
  out[11] = 0x00;

  *outlen = 12;

  return 0;
}

static uint8_t modbus_06wt_singlreg(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *wo)
{
  uint16_t startaddr;

  if(inlen < (MODBUS_FRAME_MINLEN + 4))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);

  if(startaddr > wo->hrsize)
  {
    return 1;
  }

  wo->holdreg[startaddr] = BIGENDIAN_TO_U16(in[10], in[11]);

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[4] = 0x00;
  out[5] = 0x06;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_06WT_SINGLREG;
  out[8] = (uint8_t)(startaddr >> 8);
  out[9] = (uint8_t)startaddr;
  out[10] = (uint8_t)(wo->holdreg[startaddr] >> 8);
  out[11] = (uint8_t)wo->holdreg[startaddr];

  *outlen = 12;

  return 0;
}

static uint8_t modbus_15wt_multicoil(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *wro)
{
  uint8_t  i, j;
  uint8_t  bytecnt;
  uint16_t pos;
  uint16_t startaddr;
  uint16_t quantity, quindex;

  if(inlen < (MODBUS_FRAME_MINLEN + 6))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);
  quantity  = BIGENDIAN_TO_U16(in[10], in[11]);

  if(((startaddr + quantity) > wro->cosize) || \
   (quantity > MODBUS_15WT_MULTICOIL_QUAMAX) || \
   (quantity < 1)) 
  {
    return 1;
  }

  bytecnt = in[12];
  if((quantity % 8) != 0)
  {
    if(bytecnt != ((quantity/8)+1))
    {
      return 1;
    }
  }
  else
  {
    if(bytecnt != (quantity/8))
    {
      return 1;
    }
  }

  bytecnt += 13;
  quindex = quantity;
  pos = startaddr;

  for(i=13; i<bytecnt; i++)
  {
    for(j=0; ((j<8) && (quindex > 0)); j++,quindex--)
    {
      if((in[i] & (0x01<<j)) != 0)
      {
        wro->coils[pos] = 1;
      }
      else
      {
        wro->coils[pos] = 0;
      }
      
      pos++;
    }
  }

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[4] = 0x00;
  out[5] = 0x06;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_15WT_MULTICOIL;
  out[8] = (uint8_t)(startaddr >> 8);
  out[9] = (uint8_t)startaddr;
  out[10] = (uint8_t)(quantity >> 8);
  out[11] = (uint8_t)quantity;

  *outlen = 12;

  return 0;
}

static uint8_t modbus_16wt_multireg(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen, const Modbus_MemoryMapStru *wo)
{
  uint8_t  i;
  uint8_t  bytecnt;
  uint16_t pos;
  uint16_t startaddr;
  uint16_t quantity;

  if(inlen < (MODBUS_FRAME_MINLEN + 7))
  {
    /* frame err 冗余设计, 能过了crc基本没问题 */
    return 1;
  }

  startaddr = BIGENDIAN_TO_U16(in[8], in[9]);
  quantity  = BIGENDIAN_TO_U16(in[10], in[11]);

  if(((startaddr + quantity) > wo->hrsize) || \
   (quantity > MODBUS_16WT_MULTIREG_QUAMAX) || \
   (quantity < 1)) 
  {
    return 1;
  }

  bytecnt = in[12];
  if(bytecnt != (quantity<<1))
  {
    return 1;
  }

  pos = startaddr;

  for(i=0; i<quantity; i++)
  {
    wo->holdreg[pos++] = BIGENDIAN_TO_U16(in[(2*i)+13], in[(2*i)+14]);
  }

  /* modbus tcp 事务标识符原封返回 */
  out[0] = in[0];
  out[1] = in[1];
  /* modbus tcp 协议标识符 00 00 */
  out[2] = 0x00;
  out[3] = 0x00;

  out[4] = 0x00;
  out[5] = 0x06;

  out[6] = mb_local_staddr;
  out[7] = MODBUS_FUNCODE_16WT_MULTIREG;
  out[8] = (uint8_t)(startaddr >> 8);
  out[9] = (uint8_t)startaddr;
  out[10] = (uint8_t)(quantity >> 8);
  out[11] = (uint8_t)quantity;

  *outlen = 12;

  return 0;
}

uint8_t modbus_tcp_process(const uint8_t *in, uint16_t inlen, uint8_t *out, uint16_t *outlen)
{
  uint8_t  err;

  if((in == NULL) || (out == NULL) || (outlen == NULL))
  {
    return 1;
  }

  /* TODO: check address and return 2 */

  switch(in[1])
  {
    case MODBUS_FUNCODE_01RD_COILS:
      /* TODO: flash mbmap */
      err = modbus_01rd_coils(in, inlen, out, outlen, &mb_map);
      break;
    
    case MODBUS_FUNCODE_02RD_DISINPUTS:
      /* TODO: flash mbmap */
      err = modbus_02rd_disinputs(in, inlen, out, outlen, &mb_map);
      break;
    
    case MODBUS_FUNCODE_03RD_HOLDREG:
      mb_holdreg_rd_update(&mbmmap.hr);
      err = modbus_03rd_holdreg(in, inlen, out, outlen, &mb_map);
      break;
    
    case MODBUS_FUNCODE_04RD_INPUTREG:
      mb_inputreg_rd_update(&mbmmap.ir);
      err = modbus_04rd_inputreg(in, inlen, out, outlen, &mb_map);
      break;
    
    case MODBUS_FUNCODE_05WT_SINGLCOIL:
      /* TODO: flash mbmap */
      err = modbus_05wt_singlcoil(in, inlen, out, outlen, &mb_map);
      break;
    
    case MODBUS_FUNCODE_06WT_SINGLREG:
      /* TODO: flash mbmap */
      err = modbus_06wt_singlreg(in, inlen, out, outlen, &mb_map);
      break;
    
    case MODBUS_FUNCODE_15WT_MULTICOIL:
      /* TODO: flash mbmap */
      err = modbus_15wt_multicoil(in, inlen, out, outlen, &mb_map);
      break;
    
    case MODBUS_FUNCODE_16WT_MULTIREG:
      /* TODO: flash mbmap */
      err = modbus_16wt_multireg(in, inlen, out, outlen, &mb_map);
      break;
    
    default:
      err = 3;
      break;
  }

  switch(err)
  {
    case 0:
      /* TODO: send res */
      break;

    case 1:
      /* modbus tcp 事务标识符原封返回 */
      out[0] = in[0];
      out[1] = in[1];
      /* modbus tcp 协议标识符 00 00 */
      out[2] = 0x00;
      out[3] = 0x00;

      out[4] = 0x00;
      out[5] = 0x03;

      out[6] = mb_local_staddr;
      out[7] = in[1] | 0x80u;
      out[8] = 0x02u;
    
      *outlen = 9;
      /* TODO: send res */
      break;

    case 2:
      /* modbus tcp 事务标识符原封返回 */
      out[0] = in[0];
      out[1] = in[1];
      /* modbus tcp 协议标识符 00 00 */
      out[2] = 0x00;
      out[3] = 0x00;

      out[4] = 0x00;
      out[5] = 0x03;
    
      out[6] = mb_local_staddr;
      out[7] = in[1] | 0x80u;
      out[8] = 0x03u;

      *outlen = 9;
      /* TODO: send res */
      break;

    case 3:
      /* modbus tcp 事务标识符原封返回 */
      out[0] = in[0];
      out[1] = in[1];
      /* modbus tcp 协议标识符 00 00 */
      out[2] = 0x00;
      out[3] = 0x00;

      out[4] = 0x00;
      out[5] = 0x03;
    
      out[6] = mb_local_staddr;
      out[7] = in[1] | 0x80u;
      out[8] = 0x01u;

      *outlen = 9;
      /* TODO: send res */
      break;

    default:
      return 3;
  }

  return 0;
}

void modbus_init(void)
{
  memset(&mbmmap, 0, sizeof(mbmmap));
}
